﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_OnePointError.cs
 * \brief	This file contains the OnePointError functions function that 
 *  
 * This file contains the onePointError function 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2010-02-16
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                       2010-02-16 \n
 *      First implementation of the M44_OnePointError class. Copied from the SlamPossitioning class.
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_OnePointError
     * 
     *  \brief  This is the class of the M44_OnePointError function
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_OnePointErrors
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_SlamStructures.cl_SubCellErrors OnePointErrors(cl_SlamStructures.cl_auxMapCell aux_MapCell_local_Slam,
                                                                                int[] ti_RobotCell,
                                                                                cl_SlamStructures.cl_SumErrors SumErrors,
                                                                                cl_LowResMap LowResMapGlobal,
                                                                                cl_Map2D HighResMap)
         * 
         *  \brief This function calculates the errors (gradient, height and total) for just one point
         *  
         *  \param[in] cl_SlamStructures.cl_auxMapCell aux_MapCell_local_Slam,
         *  \param[in] int[] ti_RobotCell,
         *  \param[in] cl_SlamStructures.cl_SumErrors SumErrors,
         *  \param[in] cl_Map2D HighResMap,
         *  \param[in] cl_LowResMap LowResMapGlobal,
         *  
         *  \param[out] M44_auxSlamStr.cl_SubCellErrors SubCellErrors
         */
        /* ------------------------------------------------------------------------- */

        public static M44_auxSlamStr.cl_SubCellErrors OnePointErrors(M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
                                    int[] ti_RobotCell,
                                    M44_auxSlamStr.cl_SumErrors SumErrors,
                                    cl_Map2D LowResMapGlobal,
                                    cl_Map2D HighResMap)
        {

            bool b_cond1 = false;
            bool b_cond2 = false;
            float e1, e2, e_sqr_grad, e_sqr_height, e_max_grad, e_max_height, e_total_grad, e_total_global, e_total_height;
            float[,] tf_eGX, tf_eGY, tf_eG_total, tf_eH;
            float[,] tf_LocalMap_GX, tf_LocalMap_GY, tf_LocalMap_H;
            int s_Nm1, s_Nm2;
            M44_auxSlamStr.cl_SubCellErrors SubCellErrors;

            //Capture Maps to be compared
            tf_LocalMap_GX = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(1, LowResMapGlobal, HighResMap, ti_RobotCell);
            tf_LocalMap_GY = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(2, LowResMapGlobal, HighResMap, ti_RobotCell);
            tf_LocalMap_H = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(3, LowResMapGlobal, HighResMap, ti_RobotCell);

            //Comparing the matrixes:

            s_Nm1 = HighResMap.us_Nrows;
            s_Nm2 = HighResMap.us_Ncols;
            tf_eH = new float[s_Nm1, s_Nm2];
            tf_eGX = new float[s_Nm1, s_Nm2];
            tf_eGY = new float[s_Nm1, s_Nm2];
            tf_eG_total = new float[s_Nm1, s_Nm2];
            e_max_height = 0;
            e_max_grad = 0;
            for (int i_index = 0; i_index < s_Nm1; i_index++)
            {
                for (int j_index = 0; j_index < s_Nm2; j_index++)
                {
                    if (Math.Abs(aux_MapCell_local_Slam.Measured_Heights[i_index, j_index]) < 0.01)
                    {
                        tf_LocalMap_H[i_index, j_index] = 0;
                        aux_MapCell_local_Slam.Measured_Heights[i_index, j_index] = 0;
                    }
                    b_cond1 = (Math.Abs(aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index]) < 0.0001);
                    b_cond2 = (Math.Abs(aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index]) < 0.0001);
                    if (b_cond1 && b_cond2)
                    {
                        tf_LocalMap_GX[i_index, j_index] = 0;
                        tf_LocalMap_GY[i_index, j_index] = 0;
                        aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index] = 0;
                        aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index] = 0;
                    }
                    //Maximum and Minimum error computation
                    tf_eGX[i_index, j_index] = Math.Abs(tf_LocalMap_GX[i_index, j_index] - aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index]);
                    tf_eGY[i_index, j_index] = Math.Abs(tf_LocalMap_GY[i_index, j_index] - aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index]);
                    tf_eG_total[i_index, j_index] = tf_eGX[i_index, j_index] + tf_eGY[i_index, j_index];
                    tf_eH[i_index, j_index] = Math.Abs(tf_LocalMap_H[i_index, j_index] - aux_MapCell_local_Slam.Measured_Heights[i_index, j_index]);
                    e_max_grad = Math.Max(e_max_grad, tf_eG_total[i_index, j_index]);
                    e_max_height = Math.Max(e_max_height, tf_eG_total[i_index, j_index]);

                }
            }

            //Error Computation
            // Minimum Square error based on gradient layers
            e1 = M44_auxSlamFunctions.error_tests(tf_LocalMap_GX, aux_MapCell_local_Slam.Measured_Gradient_X, s_Nm1, s_Nm2);
            e2 = M44_auxSlamFunctions.error_tests(tf_LocalMap_GY, aux_MapCell_local_Slam.Measured_Gradient_Y, s_Nm1, s_Nm2);
            e_sqr_grad = (float)Math.Sqrt((double)(Math.Pow((double)e1, 2) + (Math.Pow((double)e2, 2))));
            //Minimum square error based on height layer
            e_sqr_height = M44_auxSlamFunctions.error_tests(tf_LocalMap_H, aux_MapCell_local_Slam.Measured_Heights, s_Nm1, s_Nm2);

            //Total Error Calculation
            e_total_grad = e_sqr_grad * e_max_grad;
            e_total_height = e_sqr_height * e_max_height;
            e_total_global = (e_total_grad / SumErrors.Err_sum_grad) * (e_total_height / SumErrors.Err_sum_height);

            //Save error in exit struct
            SubCellErrors.eTotalGrad = e_total_grad;
            SubCellErrors.eTotalGlobal = e_total_global;
            SubCellErrors.eTotalHeight = e_total_height;

            return SubCellErrors;
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */